#ifndef _FAST_EXPLORATION_FSM_H_
#define _FAST_EXPLORATION_FSM_H_

#include <Eigen/Eigen>

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <std_msgs/Empty.h>
#include <std_msgs/Bool.h>
#include <std_msgs/String.h>
#include <nav_msgs/Odometry.h>
#include <visualization_msgs/Marker.h>
#include <exploration_manager/DroneState.h>
#include <exploration_manager/PairOpt.h>
#include <exploration_manager/PairOptResponse.h>
#include <bspline/Bspline.h>
#include <geometry_msgs/PointStamped.h>

#include <algorithm>
#include <iostream>
#include <vector>
#include <memory>
#include <string>
#include <thread>

using Eigen::Vector3d;
using std::shared_ptr;
using std::string;
using std::unique_ptr;
using std::vector;

namespace fast_planner {
class FastPlannerManager;
class FastExplorationManager;
class PlanningVisualization;
struct FSMParam;
struct FSMData;

enum EXPL_STATE {
  INIT,
  WAIT_TRIGGER,
  PLAN_TRAJ,
  PUB_TRAJ,
  EXEC_TRAJ,
  FINISH,
  IDLE,
  URGENT_STOP,
  PLAN_TRAJ_CA,
  PUB_TRAJ_CA,
  EXEC_TRAJ_CA
};

class FastExplorationFSM {

public:
  FastExplorationFSM(/* args */) {
  }
  ~FastExplorationFSM() {
  }

  void init(ros::NodeHandle& nh);

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

private:
  /* helper functions */
  int callExplorationPlanner();
  void transitState(EXPL_STATE new_state, string pos_call);
  void visualize(int content, bool no_trajs = false);
  void clearVisMarker();
  int getId();
  void findUnallocated(const vector<int>& actives, vector<int>& missed);
  bool riskPredict(int drone_id);
  double clip(double value, double min_val, double max_val);
  bool calculateAngleAndIntersection(const Eigen::Vector3d& A, const Eigen::Vector3d& B,
      Eigen::Vector3d next_pos_A, Eigen::Vector3d next_pos_B);

  /* ROS functions */
  void FSMCallback(const ros::TimerEvent& e);
  void safetyCallback(const ros::TimerEvent& e);
  void frontierCallback(const ros::TimerEvent& e);
  // void triggerCallback(const geometry_msgs::PoseStampedConstPtr& msg);
  void triggerCallback(const geometry_msgs::PointStampedConstPtr& msg);
  void odometryCallback(const nav_msgs::OdometryConstPtr& msg);
  void collisionCallback(const ros::TimerEvent& e);
  bool checkPositionRelativeToA(const Eigen::Vector3d& A_pos, const Eigen::Vector3d& B_pos,
      const Eigen::Vector3d& A_next_pos, double& angle, double angle_threshold_degrees = 45.0);
  bool checkPositionRelativeToA_Vel(
      const Eigen::Vector3d& B_pos, double angle_threshold_degrees = 45.0);
  bool willCollide(const Eigen::Vector3d& A_pos, const Eigen::Vector3d& A_next_pos);
  void collisionCallbackNew(const ros::TimerEvent& e);
  vector<int> findMaxUnion(vector<int>& my_cluster, vector<vector<int>>& vec);
  std::pair<double, double> calculateGroupCenter(const std::vector<int>& drones);
  int findMostCriticalDrone(const std::vector<int>& drones);
  double calculateOptimalDirection(
      int drone, const std::vector<int>& all_drones, double step_size = 0.5);
  void countCallback(const ros::TimerEvent& e);
  void envCallback(const ros::TimerEvent& e);
  void endCallback(std_msgs::String msg);

  // Swarm
  void droneStateTimerCallback(const ros::TimerEvent& e);
  void droneStateMsgCallback(const exploration_manager::DroneStateConstPtr& msg);
  void optTimerCallback(const ros::TimerEvent& e);
  void optMsgCallback(const exploration_manager::PairOptConstPtr& msg);
  void optResMsgCallback(const exploration_manager::PairOptResponseConstPtr& msg);
  void swarmTrajCallback(const bspline::BsplineConstPtr& msg);
  void swarmTrajTimerCallback(const ros::TimerEvent& e);

  /* planning utils */
  shared_ptr<FastPlannerManager> planner_manager_;
  shared_ptr<FastExplorationManager> expl_manager_;
  shared_ptr<PlanningVisualization> visualization_;

  shared_ptr<FSMParam> fp_;
  shared_ptr<FSMData> fd_;
  EXPL_STATE state_;

  /* ROS utils */
  ros::NodeHandle node_;
  ros::Timer exec_timer_, safety_timer_, vis_timer_, frontier_timer_, collision_timer_,
      count_timer_, env_timer_;
  ros::Subscriber trigger_sub_, odom_sub_, end_sub_;
  ros::Publisher replan_pub_, new_pub_, bspline_pub_, stop_pub_, land_pub_, radio_land_pub_;

  // Swarm state
  ros::Publisher drone_state_pub_, opt_pub_, opt_res_pub_, swarm_traj_pub_, grid_tour_pub_,
      hgrid_pub_;
  ros::Subscriber drone_state_sub_, opt_sub_, opt_res_sub_, swarm_traj_sub_;
  ros::Timer drone_state_timer_, opt_timer_, swarm_traj_timer_;
  bool may_collision, may_replan, time_begin, time_begin_env, land_instantly;
  ros::Time time_collision_freeze, time_collision_freeze_env;
  Eigen::Vector3d next_goal_;
  double x_min_, x_max_, y_min_, y_max_, collision_dis1_, collision_dis2_, angle1, angle2,
      end_diff_;
  int ftr_count_;
};

}  // namespace fast_planner

#endif